#include <i2c.h>
#include "smallsatos.h"
#include "ina209.h"
#include <p18f4680.h>
#include <delays.h>

OS_EVENT* ina209_query_mbox;
OS_EVENT* ina209_response_mbox;

OS_EVENT* get_ina209_query_mbox(void)
{
	return ina209_query_mbox;
}

OS_EVENT* get_ina209_response_mbox(void)
{
	return ina209_response_mbox;
}

void AckRI2C (void)
{
	while (SSPCON2bits.ACKSTAT != 0);
}


void ina209_init(int index, int config)
{
	StartI2C();
	WriteI2C(INA209_WADDR+2*index);
	WriteI2C(0x00);
	WriteI2C(config >> 8);
	WriteI2C(config & 0xFF);
	StopI2C();
}


unsigned int ina209_get (int index, commande_ina209_t addr)
{	
	INT8U value_high;
	INT8U value_low;
	INT16U value;

	StartI2C();
	WriteI2C(INA209_WADDR + 2*index);
	WriteI2C(addr);
	StopI2C();
	IdleI2C(); 
	StartI2C();
	WriteI2C(INA209_RADDR + 2*index);
    value_high = ReadI2C();
	AckI2C();
    value_low = ReadI2C();
	AckI2C();
	StopI2C();

	value = (value_high * 256 + value_low);

	return value;
	
}


void ina209_driver(void* pdata)
{
	OS_CPU_SR  cpu_sr; /* CPU Status reg. backup (required for the critical mode) */

	INT16U i;
	INT16U err;
	INT16U mes;
	ina209_query* query;

	ina209_query_mbox = OSMboxCreate(NULL);
	ina209_response_mbox = OSMboxCreate(NULL);
	
	for (;;)
	{
		query = (ina209_query*)OSMboxPend(ina209_query_mbox, 0, &err);
		OS_ENTER_CRITICAL();	
		query.value = (ina209_get(query->index, query->command));
		//query.value = 3;
		OS_EXIT_CRITICAL();
		OSMboxPost(ina209_response_mbox, query);
	}
}
